/*
 * @Description: Robot chassis control header file
 * @Version: V1.0
 * @Author: zw_1520@163.com
 * @Date: 2025-08-16 09:48:17
 * @LastEditors: zw_1520@163.com
 * @LastEditTime: 2025-08-21 09:25:36
 * Copyright (C) 2024-2050 Lens All rights reserved.
 */

#include <tf2/LinearMath/Quaternion.h>
#include <tf2_ros/transform_broadcaster.h>

#include <chrono>
#include <functional>
#include <geometry_msgs/msg/quaternion.hpp>
#include <geometry_msgs/msg/twist.hpp>
#include <memory>
#include <nav_msgs/msg/odometry.hpp>
#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/battery_state.hpp>
#include <sensor_msgs/msg/joint_state.hpp>
#include <sensor_msgs/msg/range.hpp>
#include <std_msgs/msg/float32.hpp>
#include <std_msgs/msg/int32.hpp>
#include <std_msgs/msg/string.hpp>
#include <string>

#include "chassis_kinematics.h"

// Defines motor speed scaling factor, unit: 0.0001 circle/s
#define MOTOR_SPEED_CIRCLE_SCALING_FACTOR (10000.0)

using namespace std::chrono_literals;
using std::placeholders::_1;

class ChassisController : public rclcpp::Node {
public:
  ChassisController();

private:
  // Processes chassis motor control command
  void motorControlCallback(const std_msgs::msg::Int32::SharedPtr msg) const;

  // Listens chassis motor joint state
  void chassisJointStateCallback(
      const sensor_msgs::msg::JointState::SharedPtr msg) const;

  // Listens cmd_vel topic to move chassis
  void cmdVelCallback(const geometry_msgs::msg::Twist::SharedPtr msg) const;

  // Updates odom
  void odomUpdateTimerCallback();
  void odomPublishTimerCallback();

private:
  // Defines for chassis joint state listenning
  rclcpp::Subscription<sensor_msgs::msg::JointState>::SharedPtr
      m_chassis_joint_state_control_sub_;

  // Defines for chassis control command listenning
  rclcpp::Subscription<std_msgs::msg::Int32>::SharedPtr m_motor_control_sub_;

  // Defines for chassis velocity controlling
  rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr
      m_chassis_velocity_control_sub_;

  // Defines for odom updating
  rclcpp::TimerBase::SharedPtr m_odom_update_timer_;
  rclcpp::TimerBase::SharedPtr m_get_chassis_status_timer_;
  std::chrono::high_resolution_clock::time_point m_last_calculate_time_;

  rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr m_odom_publisher_;
  std::shared_ptr<tf2_ros::TransformBroadcaster> m_tf_broadcaster_;
  rclcpp::TimerBase::SharedPtr m_odom_publish_timer_;

  // Defines communication instance pointer
  ChassisKinematics *m_chassis_kinematics_ptr_;
};